#!/usr/bin/env python
#encoding=utf-8

import sys

import rospy
import actionlib
from control_msgs.msg import (FollowJointTrajectoryAction,
                              FollowJointTrajectoryGoal,
                              GripperCommandAction,
                              GripperCommandGoal)
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

def main():
    # 设置参数
    #手臂
    arm_joint_names = ["torso_lift_joint","shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
                "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
    arm_joint_positions_get = [0.5,0.0,-0.90,0.0,1.05,0.0,1.42,1.57]
    #夹取器
    gripper_joint_names = ["l_gripper_finger_joint","r_gripper_finger_joint"]
    gripper_positions = [0.01,0.03,0.05,0.06,0.1]

    # 连接服务器
    arm_client = actionlib.SimpleActionClient("arm_with_torso_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
    arm_client.wait_for_server()
    gripper_client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction)
    gripper_client.wait_for_server()

    # 传递参数
    #手臂
    trajectory = JointTrajectory()
    trajectory.joint_names = arm_joint_names
    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = arm_joint_positions_get
    trajectory.points[0].velocities =  [0.0] * len(arm_joint_positions_get)
    trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions_get)
    trajectory.points[0].time_from_start = rospy.Duration(3.0)
    arm_goal = FollowJointTrajectoryGoal()
    arm_goal.trajectory = trajectory
    arm_goal.goal_time_tolerance = rospy.Duration(0.0)
    #夹取器
    gripper_goal = GripperCommandGoal()
    gripper_goal.command.max_effort = 30.0
    gripper_goal.command.position = gripper_positions[0]

    # 发送goal
    gripper_client.send_goal(gripper_goal)
    gripper_client.wait_for_result(rospy.Duration(0.0))
    arm_client.send_goal(arm_goal)
    arm_client.wait_for_result(rospy.Duration(0.0))


if __name__ == "__main__":
    rospy.init_node("pick_up_item")
    main()




